#ifndef SENSOR_RADLOGIC_HPP_
#define SENSOR_RADLOGIC_HPP_

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <std_msgs/Empty.h>
#include "ultra_band_wave_ladar/Sensor_mmRad.hpp"
#include "ultra_band_wave_ladar/Sensor_Ultra.hpp"
#include "ultra_band_wave_ladar/ObstacleInfo.h"

typedef struct Sensor_Comm_Info
{
  ultra_band_wave_ladar::ObstacleInfo mmRad_front;
  ultra_band_wave_ladar::ObstacleInfo UltraRad_back;
  ultra_band_wave_ladar::ObstacleInfo UltraRad_left;
  ultra_band_wave_ladar::ObstacleInfo UltraRad_right;

  uint8_t machine_st;
  uint8_t run_st;

  std::vector<std::string> array_portnames;
  std::vector<uint8_t> array_mmRad;
  std::vector<uint8_t> array_UltraRad_back;
  std::vector<uint8_t> array_UltraRad_left;
  std::vector<uint8_t> array_UltraRad_right;

  bool mmRad_Alarm;
  bool UltraRad_back_Alarm;
  bool UltraRad_left_Alarm;
  bool UltraRad_right_Alarm;
} Sensor_Comm_Info;

static Sensor_Comm_Info sensorinfo;

class Sensor_RadLogic
{
private:
  Sensor_mmRad mmRad_front;
  Sensor_Ultra UltraRad_back;
  Sensor_Ultra UltraRad_left;
  Sensor_Ultra UltraRad_right;

  std::vector<std::string> portnames;
  int SaveDist[4] = { 0, 0, 0, 0 };

public:
  Sensor_RadLogic();
  ~Sensor_RadLogic();

  void setSensorportnames(const std::vector<std::string>& names);
  void InitPara();
  bool InitPort();
  void ActiveSensor();
  void setRadMode(int type, int Var, int side);
  void set_AlarmDistance(int* Dist);
  void mmRadProcessing();
  void UltraRadProcessing();
  void ObstacleProcessing();
  void ClosePort();
};

#endif